#pragma once
#include <ros/ros.h>
#include <pcl/point_cloud.h>
#include <sensor_msgs/PointCloud2.h>
#include <pcl/point_types.h>
#include "time_utils.h"
#include "common.h"
#include "Angle.h"
#include "Vector3.h"
#include "CircularBuffer.h"
#include "math_utils.h"
#include "k_means.h"
#include "get_accessible_area.h"
#include "pcloud_scans/grid_Dz.h"
typedef std::pair<size_t, size_t> IndexRange;

class MultiScanMapper {
public:
  const float& getLowerBound() { return _lowerBound; }
  const float& getUpperBound() { return _upperBound; }  //获取上下边界   和线的数目
  const uint16_t& getNumberOfScanRings() { return _nScanRings; }

  int getRingForAngle(const float& angle);

private:
  float _lowerBound = -15;      ///< the vertical angle of the first scan ring
  float _upperBound = 15;      ///< the vertical angle of the last scan ring
  uint16_t _nScanRings = 16;   ///< number of scan rings
  float _factor = 0.5;          ///< linear interpolation factor
};


class dealScanRegistration
{
    public:
    void handleCloudMessage(const sensor_msgs::PointCloud2ConstPtr &laserCloudMsg);
    void process(pcl::PointCloud<pcl::PointXYZI>& laserCloudIn, const Time& scanTime);
    void get_line();
    dealScanRegistration(ros::NodeHandle &nh);
    
    private:
    int _systemDelay = 5;             ///< system startup delay counter
    MultiScanMapper _scanMapper;  ///< mapper for mapping vertical point angles to scan ring IDs
    std::vector<pcl::PointCloud<pcl::PointXYZI> > _laserCloudScans;
    ros::Subscriber _subLaserCloud;   ///< input cloud message subscriber
};

  
  typedef struct IMUState
  {
    Time stamp;
    Angle roll;
    Angle pitch;
    Angle yaw;
    Vector3 position;
    Vector3 velocity;
    Vector3 acceleration;

    static void interpolate(const IMUState& start,
      const IMUState& end,
      const float& ratio,
      IMUState& result)
    {
      float invRatio = 1 - ratio;

      result.roll = start.roll.rad() * invRatio + end.roll.rad() * ratio;
      result.pitch = start.pitch.rad() * invRatio + end.pitch.rad() * ratio;
      if (start.yaw.rad() - end.yaw.rad() > M_PI)
      {
        result.yaw = start.yaw.rad() * invRatio + (end.yaw.rad() + 2 * M_PI) * ratio;
      }
      else if (start.yaw.rad() - end.yaw.rad() < -M_PI)
      {
        result.yaw = start.yaw.rad() * invRatio + (end.yaw.rad() - 2 * M_PI) * ratio;
      }
      else
      {
        result.yaw = start.yaw.rad() * invRatio + end.yaw.rad() * ratio;
      }

      result.velocity = start.velocity * invRatio + end.velocity * ratio;
      result.position = start.position * invRatio + end.position * ratio;
    };
  } IMUState;

//extern pcl::PointCloud<pcl::PointXYZI> _laserCloud;   ///< full resolution input cloud



    //Time _sweepStart;            ///< time stamp of beginning of current sweep
    //Time _scanTime;              ///< time stamp of most recent scan
    //IMUState _imuStart;                     ///< the interpolated IMU state corresponding to the start time of the currently processed laser scan
    //IMUState _imuCur;                       ///< the interpolated IMU state corresponding to the time of the currently processed laser scan point
    //Vector3 _imuPositionShift;              ///< position shift between accumulated IMU position and interpolated IMU position
    //size_t _imuIdx = 0;                         ///< the current index in the IMU history
    //CircularBuffer<IMUState> _imuHistory;   ///< history of IMU states for cloud registration

   // pcl::PointCloud<pcl::PointXYZ> _imuTrans = { 4,1 };  ///< IMU transformation information

   // std::vector<float> _regionCurvature;      ///< point curvature buffer
    //std::vector<PointLabel> _regionLabel;     ///< point label buffer
    //std::vector<size_t> _regionSortIndices;   ///< sorted region indices based on point curvature
    //std::vector<int> _scanNeighborPicked;     ///< flag if neighboring point was already picked


    void updateIMUData(Vector3& acc, IMUState& newState);
    void projectPointToStartOfSweep(pcl::PointXYZI& point, float relTime);

    //auto const& imuTransform          () { return _imuTrans             ; }
    //auto const& sweepStart            () { return _sweepStart           ; }
    //auto const& laserCloud            () { return _laserCloud           ; }

    //auto const& config                () { return _config               ; }


    //inline bool hasIMUData() { return _imuHistory.size() > 0; };


    void setIMUTransformFor(const float& relTime);


    void transformToStartIMU(pcl::PointXYZI& point);


    void reset(const Time& scanTime);

    void extractFeatures(const uint16_t& beginIdx = 0);

    void setRegionBuffersFor(const size_t& startIdx,
      const size_t& endIdx);

    void setScanBuffersFor(const size_t& startIdx,
      const size_t& endIdx);

    void markAsPicked(const size_t& cloudIdx,
      const size_t& scanIdx);

    void interpolateIMUStateFor(const float& relTime, IMUState& outputState);

    void updateIMUTransform();

    void Scanlinesviewer(pcl::PointCloud<pcl::PointXYZI>::Ptr out);
    void z_deal(pcl::PointCloud<pcl::PointXYZI>::Ptr deal_data, size_t leng_size, double higt);
    void angle_deal(pcl::PointCloud<pcl::PointXYZI>::Ptr deal_data, size_t leng_size);
extern    ros::Publisher Lines_pub;
extern    ros::Publisher Scan_pub;
extern    ros::Publisher K_lines_pub;
extern    ros::Publisher PubImuTrans_pub;
extern    ros::Publisher Makerpcloud_pub;
extern    ros::Publisher Makerpcloud1_pub;
extern    ros::Publisher Access1_pub;
extern    ros::Publisher Access2_pub;
extern    ros::Publisher Access3_pub;
extern    ros::Publisher Access4_class;
extern    ros::Publisher Access5_grid;
extern  pcl::PointCloud<pcl::PointXYZI>::Ptr Obs_pointcloud;